/**
 * 
*/

import processing.serial.*;

Serial myPort;  // Create object from Serial class

boolean firstSample = true;

float [] RwAcc = new float[3];         //projection of normalized gravitation force vector on x/y/z axis, as measured by accelerometer
float [] Gyro = new float[3];          //Gyro readings
float [] RwGyro = new float[3];        //Rw obtained from last estimated value and gyro movement
float [] Awz = new float[2];           //angles between projection of R on XZ/YZ plane and Z axis (deg)
float [] RwEst = new float[3];


int boxWidth = 240;
int boxHeight = 40;
int boxDepth = 160;


//Detect Yaw changes
boolean bYawLeft = false;
boolean bYawRight = false;
int nYawLeftSamples = 0;
int nYawRightSamples = 0;

//Detect Pitch changes
boolean bPitchUp = false;
boolean bPitchDown = false;
int nPitchUpSamples = 0;
int nPitchDownSamples = 0;

//Detect roll changes
boolean bRollRight = false;
boolean bRollLeft = false;
int nRollRightSamples = 0;
int nRollLeftSamples = 0;

int lastTime = 0;
int interval = 0;
float wGyro = 10.0;

int lf = 10; // 10 is '\n' in ASCII
byte[] inBuffer = new byte[100];

PFont font;
final int VIEW_SIZE_X = 1000, VIEW_SIZE_Y = 800;
//PImage imgSkinTexture = loadImage("/Users/Pavel - Old Macbook Pro/Documents/school/computerScience/spring2011/Senior Design 2/Photos/skin_texture.gif");

void setup() 
{
  size(VIEW_SIZE_X, VIEW_SIZE_Y, P3D);
   //for comm with computer use one of 300, 1200, 2400, 4800, 9600, 14400, 19200, 28800, 38400, 57600, or 115200.
  myPort = new Serial(this, Serial.list()[0], 9600);  
  

  // The font must be located in the sketch's "data" directory to load successfully
  font = loadFont("CourierNew36.vlw"); 

}


void readSensors() {
  if(myPort.available() > 0) {
    if (myPort.readBytesUntil(lf, inBuffer) > 0) {
      String inputString = new String(inBuffer);
      String [] inputStringArr = split(inputString, ',');

      if(inputStringArr.length >= 6){
      //Rx = (AdcRx*Vref/(#ADClevels - 1) - (zerogBias)) / sensitivity (in V/g)
      float RxAcc = (float(inputStringArr[0])*3.3/1023 - 1.65)/0.325;
      float RyAcc = (float(inputStringArr[1])*3.3/1023 - 1.65)/0.325;
      float RzAcc = (float(inputStringArr[2])*3.3/1023 - 1.65)/0.325;
      RwAcc[0] = RxAcc;
      RwAcc[1] = RyAcc;
      RwAcc[2] = RzAcc;
      
      float RxGyro = (float(inputStringArr[3])*3.3/1023 - 1.23)/0.00333;
      float RyGyro = (float(inputStringArr[4])*3.3/1023 - 1.23)/0.00333;
      float RzGyro = (float(inputStringArr[5])*3.3/1023 - 1.23)/0.00333;
      Gyro[0] = RxGyro;
      Gyro[1] = RyGyro;
      Gyro[2] = RzGyro;
     // println(inputStringArr[5]);

      }
      
      //------Detect Yaw changess
      
      if(Gyro[2] < -80 && ! bYawRight){
        nYawRightSamples ++;
      }
      else if(Gyro[2] > 80 & ! bYawLeft){
        nYawLeftSamples ++;
      }
        
      if(Gyro[2] < -80 && ! bYawRight  && nYawRightSamples > 8){
      //  println("Yaw right" + " " + Gyro[2] + "\n\n");
        bYawRight = true;
        bYawLeft = false;
        nYawRightSamples = 0;
      }
      else if(Gyro[2] > 80 && ! bYawLeft && nYawLeftSamples > 8){
       // println("Yaw left" + " " + Gyro[2] + "\n\n");
        bYawLeft = true;
        bYawRight = false;     
        nYawLeftSamples = 0;
      }
      else{
        bYawLeft = false;
        bYawRight = false;
      }
      
      //-----Detect pitch changes

       if(Gyro[1] < -80 && ! bPitchDown){
        nPitchDownSamples ++;
      }
      else if(Gyro[1] > 80 & ! bPitchUp){
        nPitchUpSamples ++;
      }
        
      if(Gyro[1] > 80 && ! bPitchDown  && nPitchDownSamples > 8){
        //println("Pitch Down" + " " + Gyro[1] + "\n\n");
        bPitchDown = true;
        bPitchUp = false;
        nPitchDownSamples = 0;
      }
      else if(Gyro[1] < -80 && ! bPitchUp && nPitchUpSamples > 8){
       // println("Pitch Up" + " " + Gyro[1] + "\n\n");
        bPitchUp = true;
        bPitchDown = false;     
        nPitchUpSamples = 0;
      }
      else{

        bPitchUp = false;
        bPitchDown = false;
      }
      
      //------Detect Roll changes
      
       if(Gyro[0] < -50 && ! bRollRight){
        nRollRightSamples ++;
      }
      else if(Gyro[0] > 50 & ! bRollLeft){
        nRollLeftSamples ++;
      }
        
      if(Gyro[0] > 50 && ! bRollRight  && nRollRightSamples > 5){
        println("Roll Right" + " " + Gyro[0] + "\n\n");
        bRollRight = true;
        bRollLeft = false;
        nRollRightSamples = 0;
      }
      else if(Gyro[0] < -50 && ! bRollLeft && nRollLeftSamples > 5){
        println("Roll Left" + " " + Gyro[0] + "\n\n");
        bRollLeft = true;
        bRollRight = false;     
        nRollLeftSamples = 0;
      }
      else{

        bRollLeft = false;
        bRollRight = false;
      }
    }
  }
}


void normalize3DVec(float [] vector) {
  float R;
  R = sqrt(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]);
  vector[0] /= R;
  vector[1] /= R;  
  vector[2] /= R;  
}


float squared(float x){
  return x*x;
}


void buildRectangularPrism(float widthScale, float heightScale, float depthScale) {
  //box(60, 10, 40);
  noStroke();
 // beginShape(QUADS);
  beginShape();
  
  //Z+ (to the drawing area)

 // texture(imgSkinTexture);
  fill(#C77A58);
  vertex(-boxWidth/2*widthScale, -boxHeight/2*heightScale, boxDepth/2*depthScale);
  vertex(boxWidth/2*widthScale, -boxHeight/2*heightScale, boxDepth/2*depthScale);
  vertex(boxWidth/2*widthScale, boxHeight/2*heightScale, boxDepth/2*depthScale);
  vertex(-boxWidth/2*widthScale, boxHeight/2*heightScale, boxDepth/2*depthScale);
  endShape(CLOSE);
  
  
  beginShape();
  //Z-
  fill(#C77A58);
  //texture(imgSkinTexture);
  vertex(-boxWidth/2*widthScale, -boxHeight/2*heightScale, -boxDepth/2*depthScale);
  vertex(boxWidth/2*widthScale, -boxHeight/2*heightScale, -boxDepth/2*depthScale);
  vertex(boxWidth/2*widthScale, boxHeight/2*heightScale, -boxDepth/2*depthScale);
  vertex(-boxWidth/2*widthScale, boxHeight/2*heightScale, -boxDepth/2*depthScale);
  endShape(CLOSE);
  
  
  beginShape();
  //X-
  fill(#C77A58);
  //texture(imgSkinTexture);
  vertex(-boxWidth/2*widthScale, -boxHeight/2*heightScale, -boxDepth/2*depthScale);
  vertex(-boxWidth/2*widthScale, -boxHeight/2*heightScale, boxDepth/2*depthScale);
  vertex(-boxWidth/2*widthScale, boxHeight/2*heightScale, boxDepth/2*depthScale);
  vertex(-boxWidth/2*widthScale, boxHeight/2*heightScale, -boxDepth/2*depthScale);
  endShape(CLOSE);
  
  
  beginShape();
  //X+
  fill(#C77A58);
  //texture(imgSkinTexture);
  vertex(boxWidth/2*widthScale, -boxHeight/2*heightScale, -boxDepth/2*depthScale);
  vertex(boxWidth/2*widthScale, -boxHeight/2*heightScale, boxDepth/2*depthScale);
  vertex(boxWidth/2*widthScale, boxHeight/2*heightScale, boxDepth/2*depthScale);
  vertex(boxWidth/2*widthScale, boxHeight/2*heightScale, -boxDepth/2*depthScale);
  endShape(CLOSE);
  
  
  beginShape();
  //Y-
  fill(#C77A58);
  //texture(imgSkinTexture);
  vertex(-boxWidth/2*widthScale, -boxHeight/2*heightScale, -boxDepth/2*depthScale);
  vertex(boxWidth/2*widthScale, -boxHeight/2*heightScale, -boxDepth/2*depthScale);
  vertex(boxWidth/2*widthScale, -boxHeight/2*heightScale, boxDepth/2*depthScale);
  vertex(-boxWidth/2*widthScale, -boxHeight/2*heightScale, boxDepth/2*depthScale);
  endShape(CLOSE);
  
  
  beginShape();
  //Y+
  fill(#00ffff);
  //texture(imgSkinTexture);
  vertex(-boxWidth/2*widthScale, boxHeight/2*heightScale, -boxDepth/2*depthScale);
  vertex(boxWidth/2*widthScale, boxHeight/2*heightScale, -boxDepth/2*depthScale);
  vertex(boxWidth/2*widthScale, boxHeight/2*heightScale, boxDepth/2*depthScale);
  vertex(-boxWidth/2*widthScale, boxHeight/2*heightScale, boxDepth/2*depthScale);
  
 // endShape();
  endShape(CLOSE);
}

void drawHand() {
  //Width/depth/heightScale variables are used to scale the main rectangular prism, which is used as the palm, 
  //to get it to be approximately the relative size of a finger.  These numbers came from trial & error.
  //beginShape();
  float widthScale = 0.4;    //finger segments are .4 * length of hand. Made up number, works fine!
  float depthScale = 0.2;    //There are 4 fingers. 4*.2 = 0.8, => 20% of box depth is left for space between fingers. There are 3 spaces => 6.66% * (total box depth) space per finger.
  float heightScale = 0.9;   //Stylistic... fingers slightly thinner than hand.
  
  //Number of pixels to separate finger from hand or knuckle from knuckle.
  float knuckleSeparator = 8;
  float halfBoxWidth = boxWidth/2;
  float halfBoxDepth = boxDepth/2;
  float scaledWidth = halfBoxWidth*widthScale;
  float scaledDepth = halfBoxDepth*depthScale;

  pushMatrix();
    //Build the 'palm' of the hand, which is just a big rectangular prism.  Don't scale (i.e. scale by 1)
    buildRectangularPrism(1,1,1.1);
    
  popMatrix();
  
  //-------------------------------Code below draws fingers of hand-----------------------------------------//
  //A hand is composed of a big rectangular prism for a palm, and smaller (scaled versions of that same big rectangular prism) for fingers.
  //The fingers are composed of two rectangular prisms, one for the first section of the finger and one for the second (we don't consider the 
  //third joint in this project). Each of the finger prisms are spaced from the hand for viewing purposes to create distinct sections.

  //Draw the four fingers, two segments per finger (one closest to the palm, one fingertip)
    
  for(int i = 0; i < 4; i++){
    
    pushMatrix();
      //--------------------------Finger segment closest to palm-----------------------------
      //Draw each of four 'first' joints, i.e. closest to the hand.
      float rotation = 90*sin(.001*millis());
      float verticalShift = scaledWidth * sin(radians(rotation));
      float horizontalShift = scaledWidth - sqrt(pow(scaledWidth, 2) - pow(verticalShift, 2));
      
      translate(halfBoxWidth + knuckleSeparator + scaledWidth - horizontalShift, verticalShift ,  halfBoxDepth - scaledDepth - i*(boxDepth*depthScale  + depthScale/3*boxDepth) );
      rotateZ(radians(rotation));

      buildRectangularPrism(widthScale, heightScale, depthScale);
      //---------------------END Finger segment closest to palm--------------------------
      
      
      //--------------------------Fingertip----------------------------

      float joint2Rotation = 45*sin(.001*millis()) + 45;
      
      float verticalShift2 = scaledWidth * sin(radians(joint2Rotation));
      float horizontalShift2 = scaledWidth - sqrt(pow(scaledWidth, 2) - pow(verticalShift2, 2));
      
      translate(knuckleSeparator + 2*scaledWidth - horizontalShift2, verticalShift2 , 0);
      rotateZ(radians(joint2Rotation));
      buildRectangularPrism(widthScale, heightScale, depthScale);
      //----------------------- END Fingertip----------------------------
      
    popMatrix();

  }
  
  float thumbWidthScale = 0.4;
  float thumbHeightScale = 0.9;
  float thumbDepthScale = 0.3;
  scaledWidth = thumbWidthScale * halfBoxWidth;
  scaledDepth = thumbDepthScale * halfBoxDepth;
  pushMatrix();
  
    rotateY(radians(90));
    translate(-halfBoxWidth*1.1 - knuckleSeparator , 0, halfBoxDepth*0.6);
    
    buildRectangularPrism(thumbWidthScale, thumbHeightScale, thumbDepthScale);

  popMatrix();
  
  /*
  
  //Draw each of four second joints, i.e. further from the hand.
  for(int i = 0; i < 4; i++){
    pushMatrix();
      
      translate(halfBoxWidth + 2*knuckleSeparator + scaledWidth + 2*scaledWidth, 0 , halfBoxDepth - depthScale*halfBoxDepth - i*(boxDepth*depthScale + depthScale/3*boxDepth) );
    //  rotateZ(radians(30));
      buildRectangularPrism(widthScale, heightScale, depthScale);
    
    popMatrix();
  }*/
  
}

void createHand() {  
  pushMatrix();
  translate(400, 550, 0);
    rotateX(HALF_PI * -RwEst[0]);
    rotateZ(HALF_PI * RwEst[1]);
    drawHand();
  popMatrix();
}


void getInclination() {
  int w = 0;
  float tmpf = 0.0;
  int currentTime, signRzGyro;
  
  
  readSensors();
  normalize3DVec(RwAcc);
  
  currentTime = millis();
  interval = currentTime - lastTime;
  lastTime = currentTime;
  
  if (firstSample || Float.isNaN(RwEst[0])) { // the NaN check is used to wait for good data from the Arduino
    for(w=0;w<=2;w++) {
      RwEst[w] = RwAcc[w];    //initialize with accelerometer readings
    }
  }
  else{
    //evaluate RwGyro vector
    if(abs(RwEst[2]) < 0.1) {
      //Rz is too small and because it is used as reference for computing Axz, Ayz it's error fluctuations will amplify leading to bad results
      //in this case skip the gyro data and just use previous estimate
      for(w=0;w<=2;w++) {
        RwGyro[w] = RwEst[w];
      }
    }
    else {
      //get angles between projection of R on ZX/ZY plane and Z axis, based on last RwEst
      for(w=0;w<=1;w++){
        tmpf = Gyro[w];                        //get current gyro rate in deg/s
        tmpf *= interval / 1000.0f;                     //get angle change in deg
        Awz[w] = atan2(RwEst[w],RwEst[2]) * 180 / PI;   //get angle and convert to degrees 
        Awz[w] += tmpf;             //get updated angle according to gyro movement
      }
      
      //estimate sign of RzGyro by looking in what qudrant the angle Axz is, 
      //RzGyro is pozitive if  Axz in range -90 ..90 => cos(Awz) >= 0
      signRzGyro = ( cos(Awz[0] * PI / 180) >=0 ) ? 1 : -1;
      
      //reverse calculation of RwGyro from Awz angles, for formulas deductions see  http://starlino.com/imu_guide.html
      for(w=0;w<=1;w++){
        RwGyro[0] = sin(Awz[0] * PI / 180);
        RwGyro[0] /= sqrt( 1 + squared(cos(Awz[0] * PI / 180)) * squared(tan(Awz[1] * PI / 180)) );
        RwGyro[1] = sin(Awz[1] * PI / 180);
        RwGyro[1] /= sqrt( 1 + squared(cos(Awz[1] * PI / 180)) * squared(tan(Awz[0] * PI / 180)) );        
      }
      RwGyro[2] = signRzGyro * sqrt(1 - squared(RwGyro[0]) - squared(RwGyro[1]));
    }
    
    //combine Accelerometer and gyro readings
    for(w=0;w<=2;w++) RwEst[w] = (RwAcc[w] + wGyro * RwGyro[w]) / (1 + wGyro);

    normalize3DVec(RwEst);
  }
  
  firstSample = false;
}


void draw() {  
  directionalLight(126, 126, 126, 0, 0, -1);
  ambientLight(102, 102, 102);
  //getInclination();
  
  background(#ffffff);
  fill(#000000);
  
  textFont(font, 20);
  //float temp_decoded = 35.0 + ((float) (temp + 13200)) / 280;
  //text("temp:\n" + temp_decoded + " C", 350, 250);
  /*text("RwAcc (G):\n" + RwAcc[0] + "\n" + RwAcc[1] + "\n" + RwAcc[2] + "\ninterval: " + interval, 20, 50);
  text("Gyro (°/s):\n" + Gyro[0] + "\n" + Gyro[1] + "\n" + Gyro[2], 220, 50);
  text("Awz (°):\n" + Awz[0] + "\n" + Awz[1], 420, 50);
  text("RwGyro (°/s):\n" + RwGyro[0] + "\n" + RwGyro[1] + "\n" + RwGyro[2], 20, 180);
  text("RwEst :\n" + RwEst[0] + "\n" + RwEst[1] + "\n" + RwEst[2], 220, 180);*/
  
  // display axes
  /*pushMatrix();
    translate(450, 250, 0);
    stroke(#000000);
    scale(100, 100, 100);
    line(0,0,0,1,0,0);
    line(0,0,0,0,-1,0);
    line(0,0,0,0,0,1);
    line(0,0,0, -RwEst[0], RwEst[1], RwEst[2]);
  popMatrix();*/
  
  createHand();
}


